#include "Common.h"
#include "Frustum.h"


//------------------------------------------
Frustum::Frustum(const Matrix& proj)
{
	Init(proj);
}
//------------------------------------------
void Frustum::Init(const Matrix& combo)
{
	combo.GetFrustumNearPlane(mPlanes[0]);
	combo.GetFrustumFarPlane(mPlanes[1]);
	combo.GetFrustumLeftPlane(mPlanes[2]);
	combo.GetFrustumRightPlane(mPlanes[3]);
	combo.GetFrustumTopPlane(mPlanes[4]);
	combo.GetFrustumBottomPlane(mPlanes[5]);

	Build();
}
//------------------------------------------
void Frustum::Build()
{
	mPermutedPlanes[0] = Plane(mPlanes[0].X, mPlanes[1].X, mPlanes[2].X, mPlanes[3].X);
	mPermutedPlanes[1] = Plane(mPlanes[0].Y, mPlanes[1].Y, mPlanes[2].Y, mPlanes[3].Y);
	mPermutedPlanes[2] = Plane(mPlanes[0].Z, mPlanes[1].Z, mPlanes[2].Z, mPlanes[3].Z);
	mPermutedPlanes[3] = Plane(mPlanes[0].W, mPlanes[1].W, mPlanes[2].W, mPlanes[3].W);

	mPermutedPlanes[4] = Plane(mPlanes[4].X, mPlanes[5].X, mPlanes[5].X, mPlanes[5].X);
	mPermutedPlanes[5] = Plane(mPlanes[4].Y, mPlanes[5].Y, mPlanes[5].Y, mPlanes[5].Y);
	mPermutedPlanes[6] = Plane(mPlanes[4].Z, mPlanes[5].Z, mPlanes[5].Z, mPlanes[5].Z);
	mPermutedPlanes[7] = Plane(mPlanes[4].W, mPlanes[5].W, mPlanes[5].W, mPlanes[5].W);
}
//------------------------------------------
bool Frustum::Visible(const AxisAlignedBox& box)
{
	Vector4 center(box.getCenter());
	Vector4 extent(box.getHalfSize());
	return Visible(center, extent);
}
//------------------------------------------
bool Frustum::Visible(const BoxCenterAndExtent& box)
{
	return Visible(box.Center, box.Extent);
}
//------------------------------------------
bool Frustum::Visible(const Vector4& center, const Vector4& extent)
{
	bool Result = true;

	// Load the origin & extent
	VectorRegister Orig = VectorLoadAligned(&center);
	VectorRegister Ext = VectorLoadAligned(&extent);
	// Splat origin into 3 vectors
	VectorRegister OrigX = VectorReplicate(Orig, 0);
	VectorRegister OrigY = VectorReplicate(Orig, 1);
	VectorRegister OrigZ = VectorReplicate(Orig, 2);
	// Splat extent into 3 vectors
	VectorRegister ExtentX = VectorReplicate(Ext, 0);
	VectorRegister ExtentY = VectorReplicate(Ext, 1);
	VectorRegister ExtentZ = VectorReplicate(Ext, 2);
	// Splat the abs for the pushout calculation
	VectorRegister AbsExt = VectorAbs(Ext);
	VectorRegister AbsExtentX = VectorReplicate(AbsExt, 0);
	VectorRegister AbsExtentY = VectorReplicate(AbsExt, 1);
	VectorRegister AbsExtentZ = VectorReplicate(AbsExt, 2);
	// Since we are moving straight through get a pointer to the data
	const Plane* __restrict PermutedPlanePtr = (Plane*)&mPermutedPlanes[0];
	// Process four planes at a time until we have < 4 left
	for (int Count = 0; Count < 8; Count += 4)
	{
		// Load 4 planes that are already all Xs, Ys, ...
		VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr);
		PermutedPlanePtr++;
		VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr);
		PermutedPlanePtr++;
		VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr);
		PermutedPlanePtr++;
		VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr);
		PermutedPlanePtr++;
		// Calculate the distance (x * x) + (y * y) + (z * z) - w
		VectorRegister DistX = VectorMultiply(OrigX,PlanesX);
		VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX);
		VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY);
		VectorRegister Distance = VectorSubtract(DistZ,PlanesW);
		// Now do the push out Abs(x * x) + Abs(y * y) + Abs(z * z)
		VectorRegister PushX = VectorMultiply(AbsExtentX,VectorAbs(PlanesX));
		VectorRegister PushY = VectorMultiplyAdd(AbsExtentY,VectorAbs(PlanesY),PushX);
		VectorRegister PushOut = VectorMultiplyAdd(AbsExtentZ,VectorAbs(PlanesZ),PushY);

		// Check for completely outside
		if (VectoryAnyGreaterThan(Distance,PushOut))
		{
			Result = false;
			break;
		}
	}
	return Result;
}
//------------------------------------------
bool Frustum::Visible(const Vector3& center, float radius)
{
	bool Result = true;

	// Load the origin & radius
	VectorRegister Orig = VectorLoadFloat3(&center);
	VectorRegister VRadius = VectorLoadFloat1(&radius);
	// Splat origin into 3 vectors
	VectorRegister OrigX = VectorReplicate(Orig, 0);
	VectorRegister OrigY = VectorReplicate(Orig, 1);
	VectorRegister OrigZ = VectorReplicate(Orig, 2);
	// Since we are moving straight through get a pointer to the data
	const Plane* __restrict PermutedPlanePtr = (Plane*)&mPermutedPlanes[0];
	// Process four planes at a time until we have < 4 left
	for (int Count = 0; Count < 8; Count += 4)
	{
		// Load 4 planes that are already all Xs, Ys, ...
		VectorRegister PlanesX = VectorLoadAligned(PermutedPlanePtr);
		PermutedPlanePtr++;
		VectorRegister PlanesY = VectorLoadAligned(PermutedPlanePtr);
		PermutedPlanePtr++;
		VectorRegister PlanesZ = VectorLoadAligned(PermutedPlanePtr);
		PermutedPlanePtr++;
		VectorRegister PlanesW = VectorLoadAligned(PermutedPlanePtr);
		PermutedPlanePtr++;
		// Calculate the distance (x * x) + (y * y) + (z * z) - w
		VectorRegister DistX = VectorMultiply(OrigX,PlanesX);
		VectorRegister DistY = VectorMultiplyAdd(OrigY,PlanesY,DistX);
		VectorRegister DistZ = VectorMultiplyAdd(OrigZ,PlanesZ,DistY);
		VectorRegister Distance = VectorSubtract(DistZ,PlanesW);

		// Check for completely outside
		if (VectoryAnyGreaterThan(Distance,VRadius))
		{
			Result = false;
			break;
		}
	}
	return Result;
}
//------------------------------------------